#include "FormatConvert.h"
#include "SimpleBgSubModel.h"
#include "CodeBook.h"
#include "CvCodeBook.h"

const unsigned int width = 352;
const unsigned int height = 288;
const unsigned int frameCount = 300;

int main()
{
/*	Mat imgRGB(height, width, CV_8UC3);
	char path[80];
	unsigned int f;
	SimpleBgSubModel bgModel;
	Mat mask(height, width, CV_8UC1);
	for(f = 0; f < 10; f++)
	{
		sprintf(path,"E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor00%d.yuv", f);
		if(FormatConvert::YUV420ToRGB888(path, imgRGB))
		{
			bgModel.AccumulateBackground(imgRGB);
		}
		else
		{
			cout<<"Frame "<<f<<" error!"<<endl;
			//destroyAllWindows();
			system("Pause");
			return -1;
		}
	}
	bgModel.CreateModel(3.0, 3.0);
	cout<<"Create model successfully!"<<endl;

	while(f < frameCount)
	{
		if(f < 100)
		{
			sprintf(path, "E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor0%d.yuv", f);
		}
		else
		{
			sprintf(path, "E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor%d.yuv", f);
		}
		if(FormatConvert::YUV420ToRGB888(path, imgRGB))
		{
			bgModel.BackgroundDiff(imgRGB, mask);
			imshow("RGB",imgRGB);
			imshow("Mask", mask);
			waitKey(15);
		}
		else
		{
			cout<<"Frame "<<f<<" error!"<<endl;
			destroyAllWindows();
			system("Pause");
			return -1;
		}
		f++;
	}
*/	
/*	Mat imgRGB(height, width, CV_8UC3);
	char path[80];
	unsigned int f;
	int numPixels = width*height;
	CodeBook *codeBooks = new CodeBook[numPixels];
	Mat mask(height, width, CV_8UC1);
	for(f = 0; f < 10; f++)
	{
		sprintf(path,"E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor00%d.yuv", f);
		if(FormatConvert::YUV420ToRGB888(path, imgRGB))
		{
			for(unsigned int i = 0; i < imgRGB.rows; i++)
			{
				uchar *fi = imgRGB.data + i*imgRGB.elemSize()*imgRGB.cols;
				for(unsigned int j = 0; j < imgRGB.cols; j++)
				{
					codeBooks[i*width+j].UpdateCodeBook(fi+j*imgRGB.elemSize(), 20.0, 0.6, 1.3);
				}
			}
		}
		else
		{
			cout<<"Frame "<<f<<" error!"<<endl;
			system("Pause");
			return -1;
		}
	}
	for(int i = 0; i < numPixels; i++)
	{
		codeBooks[i].ClearStaleEntries();
	}
	cout<<"Create model successfully!"<<endl;

	while(f < frameCount)
	{
		if(f < 100)
		{
			sprintf(path, "E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor0%d.yuv", f);
		}
		else
		{
			sprintf(path, "E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor%d.yuv", f);
		}
		if(FormatConvert::YUV420ToRGB888(path, imgRGB))
		{
			for(unsigned int i = 0; i < imgRGB.rows; i++)
			{
				uchar *fi = imgRGB.data + i*imgRGB.elemSize()*imgRGB.cols;
				uchar *mi = mask.data + i*mask.elemSize()*mask.cols;
				for(unsigned int j = 0; j < imgRGB.cols; j++)
				{
					*(mi+j*mask.elemSize()) = codeBooks[i*width+j].BackgroundDiff(fi+j*imgRGB.elemSize(), 20.0, 0.6, 1.3);
				}
			}
			imshow("RGB", imgRGB);
			imshow("Mask", mask);
			waitKey(15);
		}
		else
		{
			cout<<"Frame "<<f<<" error!"<<endl;
			destroyAllWindows();
			system("Pause");
			return -1;
		}
		f++;
	}
*/
	Mat imgYUV(height, width, CV_8UC3);
	Mat imgRGB(height, width, CV_8UC3);
	char path[80];
	unsigned int f;
	int numPixels = width*height;
	CvCodeBook *codeBooks = new CvCodeBook[numPixels];
	Mat mask(height, width, CV_8UC1);
	unsigned int cbBound[CHANNELS];
	for(int n = 0; n < CHANNELS; n++)
	{
		cbBound[n] = 10;
	}
	for(f = 0; f < 10; f++)
	{
		sprintf(path,"E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor00%d.yuv", f);
		if(FormatConvert::YUV420ToYUV(path, imgYUV))
		{
			for(unsigned int i = 0; i < imgYUV.rows; i++)
			{
				uchar *fi = imgYUV.data + i*imgYUV.elemSize()*imgYUV.cols;
				for(unsigned int j = 0; j < imgYUV.cols; j++)
				{
					codeBooks[i*width+j].cvUpdateCodeBook(fi+j*imgYUV.elemSize(), cbBound, CHANNELS);
				}
			}
		}
		else
		{
			cout<<"Frame "<<f<<" error!"<<endl;
			system("Pause");
			return -1;
		}
	}
	for(int i = 0; i < numPixels; i++)
	{
		codeBooks[i].cvClearStaleEntries();
	}
	cout<<"Create model successfully!"<<endl;
	int maxMod[CHANNELS], minMod[CHANNELS];
	maxMod[0] = 10;  //Set color thresholds to default values
	minMod[0] = 10;
	maxMod[1] = 10;
	minMod[1] = 10;
	maxMod[2] = 10;
	minMod[2] = 10;
	while(f < frameCount)
	{
		if(f < 100)
		{
			sprintf(path, "E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor0%d.yuv", f);
		}
		else
		{
			sprintf(path, "E:\\TDDOWNLOAD\\hall_monitor\\hall_monitor%d.yuv", f);
		}
		if(FormatConvert::YUV420ToYUV(path, imgYUV))
		{
			cvtColor(imgYUV, imgRGB, CV_YCrCb2RGB);
			for(unsigned int i = 0; i < imgYUV.rows; i++)
			{
				uchar *fi = imgYUV.data + i*imgYUV.elemSize()*imgYUV.cols;
				uchar *mi = mask.data + i*mask.elemSize()*mask.cols;
				for(unsigned int j = 0; j < imgYUV.cols; j++)
				{
					*(mi+j*mask.elemSize()) = codeBooks[i*width+j].cvBackgroundDiff(fi+j*imgYUV.elemSize(), minMod, maxMod, CHANNELS);
				}
			}
			imshow("RGB", imgRGB);
			imshow("Mask", mask);
			waitKey(15);
		}
		else
		{
			cout<<"Frame "<<f<<" error!"<<endl;
			destroyAllWindows();
			system("Pause");
			return -1;
		}
		f++;
	}
	waitKey(0);
	destroyAllWindows();
	system("Pause");
	return 0;
}